#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float64MultiArray.h>
#include <tf/transform_datatypes.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <px4ctrl/Obstacle.h>
#include "dynamic_reconfigure/server.h"
#include "px4ctrl/PIDConfig.h"

namespace mmc_ctrl {
class PX4CtrlFSM {
   public:
    PX4CtrlFSM(const ros::NodeHandle &nh);
    ~PX4CtrlFSM();

    // 创建服务器对象
    dynamic_reconfigure::Server<px4ctrl::PIDConfig> server;
    // 创建回调对象(使用回调函数，打印修改后的参数)
    dynamic_reconfigure::Server<px4ctrl::PIDConfig>::CallbackType cbType;

    void run(const ros::TimerEvent &event);

    // 速度控制函数
    void VelCtrlCallBack(const ros::TimerEvent &event);

    // 停止函数
    void Stop(int &flag_);

    // 订阅无人机当前位置数据
    void LocalOdomCallBack(const nav_msgs::OdometryConstPtr &msg);

    // 订阅无人机当前状态
    void StateCallBack(const mavros_msgs::State::ConstPtr &msg);

    // 订阅无人机当前遥控器数据
    void RCCallBack(const mavros_msgs::RCIn::ConstPtr &msg);

    uint8_t GetCurrentState() { return current_state_.connected; }

    void PoseCtrl(double target_pose_x, double target_pose_y, double target_pose_z);

    void VelCtrl(double target_vel_x, double target_vel_y, double target_vel_z);

    void VelCtrlBody(double target_vel_x, double target_vel_y, double target_vel_z);

    void PIDCallBack(px4ctrl::PIDConfig &config, uint32_t level);

    // 订阅雷达里程计数据
    void FastLioOdometryCallBack(const nav_msgs::OdometryConstPtr &msg);

    // 订阅障碍物信息
    void ObstacleFrontCallBack(const std_msgs::Float32ConstPtr &msg);

   private:
    ros::NodeHandle nh_;

    ros::Timer vel_control_timer_;  // 声明定时器，不在构造函数中初始化
    ros::Timer run_timer_;          // 声明定时器，不在构造函数中初始化

    ros::Subscriber state_sub;
    ros::Subscriber local_pos_sub;
    ros::Subscriber rc_sub;
    ros::Subscriber slam_pose_sub_;
    ros::Subscriber obstacle_front_sub_;

    ros::Publisher vec_pub;
    ros::Publisher local_pose_pub;
    ros::Publisher target_odometry_pub;
    ros::Publisher pid_pub;
    ros::Publisher obstacle_pub;
    ros::ServiceClient arming_client;
    ros::ServiceClient set_mode_client;

    bool recieved_odometry_msg_flag_ = false;
    nav_msgs::Odometry current_odometry;
    nav_msgs::Odometry target_odometry;

    float pose_ctrl_p;
    float pose_ctrl_i;
    float pose_ctrl_d;

    float vel_ctrl_p;
    float vel_ctrl_i;
    float vel_ctrl_d;
    bool offboard_control_enable = false;

    mavros_msgs::State current_state_;
    mavros_msgs::RCIn rc_;
    bool rc_control_enable_ = false;
    double rc_x_;
    double rc_y_;
    double rc_z_;

    mavros_msgs::SetMode offb_set_mode;

    bool obstacle_stop_enable = false;
    bool obstacle_avoidance_flag_enable = false;

    double ascending_height = 5.0;
    double ascending_length = 50.0;
    geometry_msgs::Point target_point;

    double front_ = 99.9;
    double back_ = 99.9;
    double left_ = 99.9;
    double right_ = 99.9;

    double stop_x;
    double stop_y;
    double stop_z;
    int point_cloud_state;
    int stop_flag_;

    int avoidance_flag_;
    double mission_alt;
    ros::Time last_avoidance_time;
};

// 定义定时器周期的静态常量
const double TIMER_PERIOD = 1.0 / 20.0;
const double RUN_TIMER_PERIOD = 1.0 / 20.0;

// 实现构造函数
PX4CtrlFSM::PX4CtrlFSM(const ros::NodeHandle &nh) : nh_(nh) {
    // 创建定时器，使用静态回调函数，并传递 this 指针
    vel_control_timer_ =
        nh_.createTimer(ros::Duration(TIMER_PERIOD), &PX4CtrlFSM::VelCtrlCallBack, this);
    run_timer_ = nh_.createTimer(ros::Duration(RUN_TIMER_PERIOD), &PX4CtrlFSM::run, this);

    state_sub =
        nh_.subscribe<mavros_msgs::State>("/mavros/state", 10, &PX4CtrlFSM::StateCallBack, this);

    rc_sub = nh_.subscribe<mavros_msgs::RCIn>("/mavros/rc/in", 10, &PX4CtrlFSM::RCCallBack, this);

    local_pos_sub = nh_.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 10,
                                                      &PX4CtrlFSM::LocalOdomCallBack, this);

    // slam_pose_sub_ = nh_.subscribe<nav_msgs::Odometry>("/Odometry", 10,
    //                                                    &PX4CtrlFSM::FastLioOdometryCallBack,
    //                                                    this);

    obstacle_front_sub_ = nh_.subscribe<std_msgs::Float32>(
        "/mmc/obstacle_front", 10, &PX4CtrlFSM::ObstacleFrontCallBack, this);

    vec_pub =
        nh_.advertise<geometry_msgs::Twist>("/mavros/setpoint_velocity/cmd_vel_unstamped", 10);

    local_pose_pub =
        nh_.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_velocity/local", 10);

    target_odometry_pub = nh_.advertise<nav_msgs::Odometry>("/mmc/setpoint_odometry", 10);
    
    obstacle_pub = nh_.advertise<px4ctrl::Obstacle>("/mmc/obstacle", 10);

    // 使用boost::bind正确绑定PIDCallBack函数
    cbType = boost::bind(&mmc_ctrl::PX4CtrlFSM::PIDCallBack, this, _1, _2);
    // 服务器对象调用回调对象
    server.setCallback(cbType);

    // 服务的客户端（设定无人机的模式、状态）
    arming_client = nh_.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
    set_mode_client = nh_.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
    offb_set_mode.request.custom_mode = "OFFBOARD";

    stop_flag_ = 0;
    avoidance_flag_ = 0;

    nh_.getParam("pose_ctrl_p", pose_ctrl_p);
    std::cout << "pose_ctrl_p: " << pose_ctrl_p << std::endl;

    nh_.getParam("vel_ctrl_p", vel_ctrl_p);

    nh_.getParam("offboard_control_enable", offboard_control_enable);
    std::cout << "offboard_control_enable: " << offboard_control_enable << std::endl;

    nh_.getParam("rc_control_enable_", rc_control_enable_);
    std::cout << "rc_control_enable_: " << rc_control_enable_ << std::endl;

    nh_.getParam("obstacle_stop_enable", obstacle_stop_enable);
    std::cout << "obstacle_stop_enable: " << obstacle_stop_enable << std::endl;

    nh_.getParam("obstacle_avoidance_enable", obstacle_avoidance_flag_enable);
    std::cout << "obstacle_avoidance_enable: " << obstacle_avoidance_flag_enable << std::endl;

    nh_.getParam("ascending_height", ascending_height);
    std::cout << "ascending_height: " << ascending_height << std::endl;
    if (ascending_height < 5.0) {
        ascending_height = 5.0;
    }

    nh_.getParam("ascending_length", ascending_length);
    std::cout << "ascending_length: " << ascending_length << std::endl;
    if (ascending_length < 50.0) {
        ascending_length = 50.0;
    }

    pid_pub = nh_.advertise<std_msgs::Float64MultiArray>("error_values", 10);
}

PX4CtrlFSM::~PX4CtrlFSM() {}

void PX4CtrlFSM::PIDCallBack(px4ctrl::PIDConfig &config, uint32_t level) {
    // ROS_INFO("动态参数");
    pose_ctrl_p = config.p;
    pose_ctrl_i = config.i;
    pose_ctrl_d = config.d;

    if (offboard_control_enable == true) {
        target_odometry.pose.pose.position.x = config.px;
        target_odometry.pose.pose.position.y = config.py;
        target_odometry.pose.pose.position.z = config.pz;
    }
}

// 位置回调函数
void PX4CtrlFSM::LocalOdomCallBack(const nav_msgs::OdometryConstPtr &msg) {
    recieved_odometry_msg_flag_ = true;
    current_odometry.header.frame_id = "odom";
    current_odometry.child_frame_id = "base_link";
    ros::Time current_time;
    current_time = ros::Time::now();
    current_odometry.header.stamp = current_time;
    current_odometry.pose.pose.position = msg->pose.pose.position;
    current_odometry.pose.pose.orientation = msg->pose.pose.orientation;
    current_odometry.twist.twist.linear = msg->twist.twist.linear;
}

void PX4CtrlFSM::FastLioOdometryCallBack(const nav_msgs::OdometryConstPtr &msg) {
    recieved_odometry_msg_flag_ = true;
    current_odometry.header.frame_id = "odom";
    current_odometry.child_frame_id = "base_link";
    ros::Time current_time;
    current_time = ros::Time::now();
    current_odometry.header.stamp = current_time;
    current_odometry.pose.pose.position = msg->pose.pose.position;
    current_odometry.pose.pose.orientation = msg->pose.pose.orientation;
    current_odometry.twist.twist.linear = msg->twist.twist.linear;
}

void PX4CtrlFSM::ObstacleFrontCallBack(const std_msgs::Float32ConstPtr &msg) {
    front_ = msg->data;
    if (front_ < 35.0) {
        last_avoidance_time = ros::Time::now();
    }
}

void PX4CtrlFSM::StateCallBack(const mavros_msgs::State::ConstPtr &msg) {
    current_state_ = *msg;
}

void PX4CtrlFSM::RCCallBack(const mavros_msgs::RCIn::ConstPtr &msg) {
    // 可以在这里接受遥控器的通道从而去做不同的事情
    rc_ = *msg;

    rc_x_ = rc_.channels[1] / 100.0 - 15.0;
    rc_y_ = rc_.channels[0] / 100.0 - 15.0;
    rc_z_ = rc_.channels[2] / 100.0 - 15.0;
    if (rc_control_enable_) {
        target_odometry.pose.pose.position.x = rc_x_ * 0.1;
        target_odometry.pose.pose.position.y = -rc_y_ * 0.1;
        target_odometry.pose.pose.position.z = rc_z_ * 0.1;
    }
}

void PX4CtrlFSM::run(const ros::TimerEvent &event) {
    // ROS_INFO("PX4CtrlFSM run!");
    tf::Quaternion q;
    tf::quaternionMsgToTF(current_odometry.pose.pose.orientation, q);
    // double roll, pitch, yaw;
    // tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
    // std::cout << "\r\nroll: " << roll << " pitch: " << pitch << " yaw: " << yaw << std::endl;

    // get quqternion inverse
    tf::Quaternion q_body_to_local = q.inverse();
    // tf::Vector3 body_vector(50, 0, 0);
    // tf::Vector3 local_vector = tf::quatRotate(q_body_to_local, body_vector);
    // std::cout << "\r\nx: " << local_vector.getX() << " y: " << local_vector.getY() << " Z: " <<
    // local_vector.getZ() << std::endl;

    // 障碍物刹停
    if (obstacle_stop_enable == true) {
        std::cout << "\r\n前: " << front_ << std::endl;
        if (front_ < 15 && stop_flag_ == 0) {
            stop_flag_ = 1;
            stop_x = current_odometry.pose.pose.position.x;
            stop_y = current_odometry.pose.pose.position.y;
            stop_z = current_odometry.pose.pose.position.z;
        } else if (stop_flag_ == 1) {
            Stop(stop_flag_);
        } else if (stop_flag_ == 2) {
            if (front_ > 15.5) {
                stop_flag_ = 0;
                ROS_INFO("Stop exit");
            }
        }
    }

    // 障碍物避障 
    if (obstacle_avoidance_flag_enable == true && current_odometry.pose.pose.position.z > 2.0) {
        if (front_ < 35.0 && avoidance_flag_ == 0) {
            avoidance_flag_ = 1;
            // 记录当前航线高度
            mission_alt = current_odometry.pose.pose.position.z;
            // std::cout << "\r\navoidance_flag_: " << avoidance_flag_ << std::endl;
        } else if (avoidance_flag_ == 1) {
            // avoidance_flag_ == 1 时，无人机执行刹停
            if (current_state_.mode != "OFFBOARD") {
                offb_set_mode.request.custom_mode = "OFFBOARD";
                set_mode_client.call(offb_set_mode);
            }
            if (current_odometry.twist.twist.linear.x > 0.5) { 
                tf::Vector3 body_vector(-1.0, 0, 0);
                tf::Vector3 local_vector = tf::quatRotate(q_body_to_local, body_vector);
                // std::cout << "body_vector x: " << body_vector.x() << "\ty: " << body_vector.y()
                //           << "\tz: " << body_vector.z() << std::endl;
                // std::cout << "loca_vector x: " << local_vector.x() << "\ty: " << local_vector.y()
                //           << "\tz: " << local_vector.z() << std::endl;
                // std::cout << "\r\nlinear x: " << current_odometry.twist.twist.linear.x << std::endl;
                VelCtrlBody(body_vector.x(), body_vector.y(), 0);
                stop_x = current_odometry.pose.pose.position.x;
                stop_y = current_odometry.pose.pose.position.y;
                stop_z = current_odometry.pose.pose.position.z;

                // std::cout << "target_odometry.twist.twist: " <<
                // target_odometry.twist.twist.linear;

            } else {
                // std::cout << "\r\nstop_x: " << stop_x << "\tstop_y: " << stop_y
                //           << "\tstop_z: " << stop_z << std::endl;
                // PoseCtrl(stop_x, stop_y, stop_z);
                VelCtrlBody(0, 0, 0);
                if (fabs(current_odometry.twist.twist.linear.x) < 0.3 &&
                    fabs(current_odometry.twist.twist.linear.y) < 0.3 &&
                    fabs(current_odometry.twist.twist.linear.z) < 0.3) {
                    avoidance_flag_ = 2;
                }
            }
            geometry_msgs::Twist target_vel;
            target_vel = target_odometry.twist.twist;
            vec_pub.publish(target_vel);
            // std::cout << "\r\navoidance_flag_: " << avoidance_flag_ << std::endl;
        } else if (avoidance_flag_ == 2) {  
            // avoidance_flag_ == 2 时，无人机上升，直到没有障碍物
            if (current_state_.mode != "OFFBOARD") {
                set_mode_client.call(offb_set_mode);
            }
            VelCtrl(0, 0, 2.0);
            // std::cout << "\r\navoidance_flag_: " << avoidance_flag_ << std::endl;
            ros::Time current_time;
            current_time = ros::Time::now();
            int last_avoidance_time_to_now = current_time.sec - last_avoidance_time.sec;  // sec

            if (last_avoidance_time_to_now > 3.0) {
                avoidance_flag_ = 3;
                target_point.x = current_odometry.pose.pose.position.x;
                target_point.y = current_odometry.pose.pose.position.y;
                target_point.z = current_odometry.pose.pose.position.z + ascending_height;
            }
            // std::cout << "\nlast_avoidance_time: " << last_avoidance_time.sec << std::endl;
            // std::cout << "\nlast_avoidance_time_to_now: " << last_avoidance_time_to_now
            //           << std::endl;
            geometry_msgs::Twist target_vel;
            target_vel = target_odometry.twist.twist;
            vec_pub.publish(target_vel);
        } else if (avoidance_flag_ == 3) {
            // avoidance_flag_ == 3 时, 无人机定点
            if (current_state_.mode != "OFFBOARD") {
                set_mode_client.call(offb_set_mode);
            }
            // std::cout << "\r\nx: " << target_point.x << "\ty: " << target_point.y
            //           << "\tz: " << target_point.z << std::endl;
            PoseCtrl(target_point.x, target_point.y, target_point.z);

            // std::cout << "\r\navoidance_flag_: " << avoidance_flag_ << std::endl;
            geometry_msgs::Twist target_vel;
            target_vel = target_odometry.twist.twist;
            vec_pub.publish(target_vel);

            if (fabs(target_point.z - current_odometry.pose.pose.position.z) < 1.0) {
                avoidance_flag_ = 4;

                tf::Vector3 body_vector(ascending_length, 0, 0);
                tf::Vector3 local_vector = tf::quatRotate(q_body_to_local, body_vector);

                target_point.x = current_odometry.pose.pose.position.x + local_vector.x();
                target_point.y = current_odometry.pose.pose.position.y - local_vector.y();
                target_point.z = current_odometry.pose.pose.position.z;
            }
        } else if (avoidance_flag_ == 4) {
            // avoidance_flag_ == 4 时, 无人机飞到目标点
            if (current_state_.mode != "OFFBOARD") {
                set_mode_client.call(offb_set_mode);
            }
            // std::cout << "\r\nx: " << target_point.x << "\ty: " << target_point.y
            //           << "\tz: " << target_point.z << std::endl;
            PoseCtrl(target_point.x, target_point.y, target_point.z);

            // std::cout << "\r\navoidance_flag_: " << avoidance_flag_ << std::endl;
            geometry_msgs::Twist target_vel;
            target_vel = target_odometry.twist.twist;
            vec_pub.publish(target_vel);

            if (fabs(target_point.x - current_odometry.pose.pose.position.x) < 0.5 &&
                fabs(target_point.y - current_odometry.pose.pose.position.y) < 0.5 &&
                fabs(target_point.z - current_odometry.pose.pose.position.z) < 0.5) {
                avoidance_flag_ = 5;
                target_point.x = current_odometry.pose.pose.position.x;
                target_point.y = current_odometry.pose.pose.position.y;
                target_point.z = mission_alt;
            }
        } else if (avoidance_flag_ == 5) {
            // avoidance_flag_ == 5 时, 无人机下降到航线高度
            if (current_state_.mode != "OFFBOARD") {
                set_mode_client.call(offb_set_mode);
            }
            // std::cout << "\r\nx: " << target_point.x << "\ty: " << target_point.y
            //           << "\tz: " << target_point.z << std::endl;
            PoseCtrl(target_point.x, target_point.y, target_point.z);

            // std::cout << "\r\navoidance_flag_: " << avoidance_flag_ << std::endl;
            // std::cout << "\r\ndiff: "
            //           << fabs(target_point.z - current_odometry.pose.pose.position.z) << std::endl;
            geometry_msgs::Twist target_vel;
            target_vel = target_odometry.twist.twist;
            vec_pub.publish(target_vel);

            if (fabs(target_point.z - current_odometry.pose.pose.position.z) < 10.0) {
                ros::Time current_time;
                current_time = ros::Time::now();
                int last_avoidance_time_to_now = current_time.sec - last_avoidance_time.sec;  // sec

                    // std::cout << "last_avoidance_time_to_now: " << last_avoidance_time_to_now
                    //         << std::endl;
                if (last_avoidance_time_to_now > 3.0) {
                    avoidance_flag_ = 6;
                }
            }
        } else if (avoidance_flag_ == 6) {
            // avoidance_flag_ == 6 时, 无人机切换到自动飞行模式
            if (current_state_.mode != "AUTO.MISSION") {
                offb_set_mode.request.custom_mode = "AUTO.MISSION";
                set_mode_client.call(offb_set_mode);
            } else if (current_state_.mode == "AUTO.MISSION") {
                avoidance_flag_ = 0;
            }
        }
    }

    target_odometry_pub.publish(target_odometry);

    if (offboard_control_enable == true) {
        if (current_state_.mode != "OFFBOARD") {
            set_mode_client.call(offb_set_mode);
        }

        PoseCtrl(target_odometry.pose.pose.position.x, target_odometry.pose.pose.position.y,
                 target_odometry.pose.pose.position.z);
        geometry_msgs::Twist target_vel;
        target_vel = target_odometry.twist.twist;
        vec_pub.publish(target_vel);
    }

    px4ctrl::Obstacle obstacle;
    obstacle.header.stamp = ros::Time::now();
    obstacle.header.frame_id = "odom";
    obstacle.front = front_;
    obstacle.back = back_;
    obstacle.left = left_;
    obstacle.right = right_;
    obstacle.avoidance_flag = avoidance_flag_;
    obstacle.setpoint_x = target_point.x;
    obstacle.setpoint_y = target_point.y;
    obstacle.setpoint_z = target_point.z;
    obstacle.setpoint_vx = target_odometry.twist.twist.linear.x;
    obstacle.setpoint_vy = target_odometry.twist.twist.linear.y;
    obstacle.setpoint_vz = target_odometry.twist.twist.linear.z;
    obstacle_pub.publish(obstacle);
}

void PX4CtrlFSM::PoseCtrl(double target_pose_x, double target_pose_y, double target_pose_z) {
    double error_x = 0;
    double error_y = 0;
    double error_z = 0;
    double error_diff_x;
    double error_diff_y;
    double error_diff_z;
    static double last_error_x = 0.0;
    static double last_error_y = 0.0;
    static double last_error_z = 0.0;
    static double error_integral_x = 0.0;
    static double error_integral_y = 0.0;
    static double error_integral_z = 0.0;

    error_x = target_pose_x - current_odometry.pose.pose.position.x;
    error_y = target_pose_y - current_odometry.pose.pose.position.y;
    error_z = target_pose_z - current_odometry.pose.pose.position.z;

    // if (error_x > 5.0) error_x = 5.0;
    // if (error_x < -5.0) error_x = -5.0;

    // if (error_y > 5.0) error_y = 5.0;
    // if (error_y < -5.0) error_y = -5.0;

    // if (error_z > 5.0) error_z = 5.0;
    // if (error_z < -5.0) error_z = -5.0;

    error_integral_x += error_x;
    error_integral_y += error_y;
    error_integral_z += error_z;

    error_diff_x = error_x - last_error_x;
    error_diff_y = error_y - last_error_y;
    error_diff_z = error_z - last_error_z;

    if (error_integral_x > 100.0) error_integral_x = 100.0;
    if (error_integral_x < -100.0) error_integral_x = -100.0;

    if (error_integral_y > 100.0) error_integral_y = 100.0;
    if (error_integral_y < -100.0) error_integral_y = -100.0;

    if (error_integral_z > 100.0) error_integral_z = 100.0;
    if (error_integral_z < -100.0) error_integral_z = -100.0;

    target_odometry.twist.twist.linear.x = error_x * pose_ctrl_p + error_integral_x * pose_ctrl_i +
                                           (error_x - last_error_x) * pose_ctrl_d;
    target_odometry.twist.twist.linear.y = error_y * pose_ctrl_p + error_integral_y * pose_ctrl_i +
                                           (error_y - last_error_y) * pose_ctrl_d;
    target_odometry.twist.twist.linear.z = error_z * pose_ctrl_p + error_integral_z * pose_ctrl_i +
                                           (error_z - last_error_z) * pose_ctrl_d;

    if (target_odometry.twist.twist.linear.x > 10.0) target_odometry.twist.twist.linear.x = 10.0;
    if (target_odometry.twist.twist.linear.x < -10.0) target_odometry.twist.twist.linear.x = -10.0;

    if (target_odometry.twist.twist.linear.y > 10.0) target_odometry.twist.twist.linear.y = 10.0;
    if (target_odometry.twist.twist.linear.y < -10.0) target_odometry.twist.twist.linear.y = -10.0;

    if (target_odometry.twist.twist.linear.z > 10.0) target_odometry.twist.twist.linear.z = 10.0;
    if (target_odometry.twist.twist.linear.z < -10.0) target_odometry.twist.twist.linear.z = -10.0;

    last_error_x = error_x;
    last_error_y = error_y;
    last_error_z = error_z;

    std_msgs::Float64MultiArray msg;
    msg.data.push_back(error_x);
    msg.data.push_back(error_integral_x);
    msg.data.push_back(error_diff_x);
    msg.data.push_back(error_y);
    msg.data.push_back(error_integral_y);
    msg.data.push_back(error_diff_y);
    msg.data.push_back(error_z);
    msg.data.push_back(error_integral_z);
    msg.data.push_back(error_diff_z);

    pid_pub.publish(msg);

    // std::cout << "------------->PoseCtrl" << std::endl;
    // std::cout << "error x: " << error_x << "\t error_d: " << error_diff_x
    //           << "\tz error_i: " << error_integral_x << std::endl;
}

void PX4CtrlFSM::VelCtrl(double target_vel_x, double target_vel_y, double target_vel_z) {
    double error_x = 0.0;
    double error_y = 0.0;
    double error_z = 0.0;

    error_x = target_vel_x - current_odometry.twist.twist.linear.x;
    error_y = target_vel_y - current_odometry.twist.twist.linear.y;
    error_z = target_vel_z - current_odometry.twist.twist.linear.z;

    // target_odometry.twist.twist.linear.x = error_x * vel_ctrl_p;
    // target_odometry.twist.twist.linear.y = error_y * vel_ctrl_p;
    // target_odometry.twist.twist.linear.z = error_z * vel_ctrl_p;

    target_odometry.twist.twist.linear.x = target_vel_x;
    target_odometry.twist.twist.linear.y = target_vel_y;
    target_odometry.twist.twist.linear.z = target_vel_z;

    // std::cout << "tar_v x: " << target_odometry.twist.twist.linear.x
    //           << "\ty: " << target_odometry.twist.twist.linear.y
    //           << "\tz: " << target_odometry.twist.twist.linear.z << std::endl;
}

void PX4CtrlFSM::VelCtrlBody(double target_vel_x, double target_vel_y, double target_vel_z) {
    tf::Quaternion q;
    tf::quaternionMsgToTF(current_odometry.pose.pose.orientation, q);
    tf::Quaternion q_body_to_local = q.inverse();

    tf::Vector3 body_vel(target_vel_x, target_vel_y, target_vel_z);
    tf::Vector3 local_vector = tf::quatRotate(q, body_vel);

    target_odometry.twist.twist.linear.x = local_vector.x();
    target_odometry.twist.twist.linear.y = local_vector.y();
    target_odometry.twist.twist.linear.z = local_vector.z();

    // std::cout << "tar_v x: " << target_odometry.twist.twist.linear.x
    //           << "\ty: " << target_odometry.twist.twist.linear.y
    //           << "\tz: " << target_odometry.twist.twist.linear.z << std::endl;
}

// 速度定时器控制函数
void PX4CtrlFSM::VelCtrlCallBack(const ros::TimerEvent &event) {}

// 刹停
void PX4CtrlFSM::Stop(int &flag_) {
    ROS_INFO("Stop! rx: %lf", rc_x_);
    if (current_state_.mode != "OFFBOARD") {
        set_mode_client.call(offb_set_mode);
    }

    if (current_odometry.twist.twist.linear.x > 0.5) {
        VelCtrl(-1.0, 0, 0);
        stop_x = current_odometry.pose.pose.position.x;
        stop_y = current_odometry.pose.pose.position.y;
    } else {
        PoseCtrl(stop_x, stop_y, stop_z);
    }
    geometry_msgs::Twist target_vel;
    target_vel = target_odometry.twist.twist;
    vec_pub.publish(target_vel);

    if (rc_x_ < -0.1) {
        flag_ = 2;
    }
}

}  // namespace mmc_ctrl

int main(int argc, char **argv) {
    ros::init(argc, argv, "px4ctrl");
    ros::NodeHandle nh("px4ctrl");
    mmc_ctrl::PX4CtrlFSM fsm(nh);
    ROS_INFO("px4ctrl node started!");
    ros::spin();

    return 0;
}